#include<iostream>
#include "../include/calib_odom/Odom_Calib.hpp"


//设置数据长度,即多少数据计算一次
void OdomCalib::Set_data_len(int len) {
    data_len = len;
    A.conservativeResize(len * 3, 9);
    b.conservativeResize(len * 3);
    A.setZero();
    b.setZero();
}


/*
输入:里程计和激光数据

构建最小二乘需要的超定方程组
Ax = b

*/
bool OdomCalib::Add_Data(Eigen::Vector3d Odom, Eigen::Vector3d scan) {


    if (now_len < INT_MAX && !is_full()) {
        // 构建超定方程组

        //for b
        for (int i = now_len * 3; i < now_len * 3 + 3; i++) {
            b(i) = scan(i - now_len * 3);
        }
        //std::cout << "b:" << b << std::endl;
        //for a
        A(now_len * 3, 0) = Odom(0);
        A(now_len * 3, 1) = Odom(1);
        A(now_len * 3, 2) = Odom(2);

        A(now_len * 3 + 1, 3) = Odom(0);
        A(now_len * 3 + 1, 4) = Odom(1);
        A(now_len * 3 + 1, 5) = Odom(2);

        A(now_len * 3 + 2, 6) = Odom(0);
        A(now_len * 3 + 2, 7) = Odom(1);
        A(now_len * 3 + 2, 8) = Odom(2);

       // std::cout << "A:" << A << std::endl;

        //end
        now_len++;
        return true;
    } else {
        return false;
    }
}

/*
 *
 * 求解线性最小二乘Ax=b
 * 返回得到的矫正矩阵
*/
Eigen::Matrix3d OdomCalib::Solve() {
    std::cout << "Solve" << std::endl;
    Eigen::Matrix3d correct_matrix;
    //Eigen::Matrix
    //:求解线性最小二乘

    //matrix transpose
    Eigen::MatrixXd AT = A.transpose();
    //std::cout << "AT:" << AT << std::endl;
    Eigen::MatrixXd x = ((AT * A).inverse()) * AT * b;
    //std::cout << "x:" << x.size() << std::endl;

    // 9x1 to 3x3
    for (int i = 0; i < 3; i++) {
        for (int j = 0; j < 3; j++) {
            correct_matrix(i, j) = x(i * 3 + j);
        }
    }

    //end

    return correct_matrix;
}

/* 用于判断数据是否满
 * 数据满即可以进行最小二乘计算
*/
bool OdomCalib::is_full() {
    if (now_len % data_len == 0 && now_len >= 1) {
        now_len = data_len;
        return true;
    } else
        return false;
}

/*
 * 数据清零
*/
void OdomCalib::set_data_zero() {
    A.setZero();
    b.setZero();
}
